{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# SL4\n",
        "\n",
        "The `SL4` class represents an element of the Special Linear Group SL(4), which is the group of all 4x4 real matrices with a determinant of 1. This group is fundamental in projective geometry, where it can represent transformations like 3D homographies.\n",
        "\n",
        "Unlike `Pose3`, which represents rigid-body motions in Euclidean space (the group SE(3)), `SL4` transformations can include shearing and non-uniform scaling, and they do not necessarily preserve distances, angles, or parallelism. It is a 15-parameter Lie group. For users familiar with computer vision, this is the group of 3D projective transformations, a.k.a. homographies."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/SL4.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import SL4\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization and Properties\n",
        "\n",
        "An `SL4` element can be initialized in several ways:\n",
        "- With no arguments to create an identity transform (a 4x4 identity matrix).\n",
        "- From a 4x4 `numpy` matrix. GTSAM will automatically normalize the matrix to ensure its determinant is 1. This is done by dividing the matrix by $\\sqrt[4]{\\det(M)}$. Note that the determinant must be positive."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Identity:\n",
            "[[1. 0. 0. 0.]\n",
            " [0. 1. 0. 0.]\n",
            " [0. 0. 1. 0.]\n",
            " [0. 0. 0. 1.]]\n",
            "\n",
            "Original Matrix (det=16):\n",
            "[[2. 0. 0. 0.]\n",
            " [0. 2. 0. 0.]\n",
            " [0. 0. 2. 0.]\n",
            " [0. 0. 0. 2.]]\n",
            "\n",
            "S1 Matrix (normalized, det=1.0):\n",
            "[[1. 0. 0. 0.]\n",
            " [0. 1. 0. 0.]\n",
            " [0. 0. 1. 0.]\n",
            " [0. 0. 0. 1.]]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Identity transform\n",
        "s_identity = gtsam.SL4()\n",
        "print(f\"Identity:\\n{s_identity.matrix()}\\n\")\n",
        "\n",
        "# Create from a numpy matrix\n",
        "# Note that det(M) = 16. SL4 will normalize this.\n",
        "M = 2 * np.eye(4)\n",
        "print(f\"Original Matrix (det=16):\\n{M}\\n\")\n",
        "S1 = SL4(M / np.power(np.linalg.det(M), 1.0/4.0))\n",
        "print(f\"S1 Matrix (normalized, det={np.linalg.det(S1.matrix()):.1f}):\\n{np.round(S1.matrix(), 3)}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Group Operations\n",
        "\n",
        "`SL4` implements the standard Lie group operations: `Identity`, `inverse`, and `compose` (via the `*` operator)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "S2:\n",
            "[[ 0.973 -0.486  0.     0.973]\n",
            " [ 0.486  0.843  0.     0.973]\n",
            " [ 0.     0.     0.973  0.   ]\n",
            " [ 0.     0.     0.     0.973]]\n",
            "\n",
            "S1.inverse():\n",
            "[[ 1. -0.  0. -0.]\n",
            " [-0.  1. -0.  0.]\n",
            " [ 0. -0.  1. -0.]\n",
            " [-0.  0. -0.  1.]]\n",
            "\n",
            "S1 * S2:\n",
            "[[ 0.973 -0.486  0.     0.973]\n",
            " [ 0.486  0.843  0.     0.973]\n",
            " [ 0.     0.     0.973  0.   ]\n",
            " [ 0.     0.     0.     0.973]]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create a second transform\n",
        "S2_matrix = np.array([\n",
        "    [1, -0.5, 0, 1],\n",
        "    [0.5, 0.866, 0, 1],\n",
        "    [0, 0, 1, 0],\n",
        "    [0, 0, 0, 1]\n",
        "])\n",
        "S2 = SL4(S2_matrix / np.power(np.linalg.det(S2_matrix), 1.0/4.0))\n",
        "print(f\"S2:\\n{np.round(S2.matrix(), 3)}\\n\")\n",
        "\n",
        "# Inverse\n",
        "s_inv = S1.inverse()\n",
        "print(f\"S1.inverse():\\n{np.round(s_inv.matrix(), 3)}\\n\")\n",
        "\n",
        "# Composition (group product)\n",
        "s_composed = S1 * S2\n",
        "print(f\"S1 * S2:\\n{np.round(s_composed.matrix(), 3)}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Lie Algebra\n",
        "\n",
        "The Lie algebra of SL(4), denoted `sl(4)`, is the tangent space at the identity. It is represented by a 15-dimensional vector `xi`. This vector can be mapped to a 4x4 matrix in the Lie algebra using the `Hat` operator; these algebra matrices are characterized by having a trace of zero. The `Expmap` and `Logmap` functions convert between the 15-D Lie algebra vector and the Lie group `SL4`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Logmap(S1) = [ 0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0. -0.]\n",
            "\n",
            "Expmap(xi):\n",
            "[[1. 0. 0. 0.]\n",
            " [0. 1. 0. 0.]\n",
            " [0. 0. 1. 0.]\n",
            " [0. 0. 0. 1.]]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Logarithm map: from group to Lie algebra\n",
        "xi_log = SL4.Logmap(S1)\n",
        "print(f\"Logmap(S1) = {np.round(xi_log, 3)}\\n\")\n",
        "\n",
        "# Exponential map: from Lie algebra to group\n",
        "S_exp = SL4.Expmap(xi_log)\n",
        "print(f\"Expmap(xi):\\n{np.round(S_exp.matrix(), 3)}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The `Hat` and `Vee` operators convert between the 15-dimensional vector `xi` and the 4x4 matrix representation of the Lie algebra `sl(4)`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Hat(xi):\n",
            "[[ 0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.]\n",
            " [ 0.  0. -0.  0.]\n",
            " [ 0.  0.  0.  0.]]\n",
            "\n",
            "Vee(X): [ 0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0.  0. -0.]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Hat: from 15D vector to 4x4 Lie algebra matrix\n",
        "X_hat = SL4.Hat(xi_log)\n",
        "print(f\"Hat(xi):\\n{np.round(X_hat, 3)}\\n\")\n",
        "\n",
        "# Vee: from 4x4 Lie algebra matrix to 15D vector\n",
        "xi_vee = SL4.Vee(X_hat)\n",
        "print(f\"Vee(X): {np.round(xi_vee, 3)}\\n\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}